import scc32
import arm
import cynematic
import time

s = scc32.SCC32( port = "/dev/tty.usbserial",
                 baudrate = 115000,
                 simulation = False)

c = cynematic.Cynematic( 64, 50, 154, 154, 100 )

a = arm.Arm( s, c )

# a.shoulder(120)

a.park()

# auto = False
auto = True 

try: 
    while True :
        a.goto(115, -110, 50, delay = 2000 )
    
        a.goto(115, -110, 20, delay = 1000 )

        a.grip(10)

        a.goto(115, -110, 50, delay = 1000 )

        a.goto(118, 88, 50, delay = 2000 )

        a.goto(118, 88, 20, delay = 1000 )

        a.grip(30, 1000)

        a.goto(118, 88, 50, delay = 1000 )

        a.goto(254,0,218,0,delay = 2000)

        if auto : 
            time.sleep(1)
        else :
            input()

        a.goto(118, 88, 50, delay = 1000 )

        a.goto(118, 88, 20, delay = 1000 )

        a.grip(10, 1000)

        a.goto(118, 88, 50, delay = 1000 )

        a.goto(115, -110, 50, delay = 2000 )

        a.goto(115, -110, 20, delay = 1000 )

        a.grip(30)

        a.goto(115, -110, 50, delay = 1000 )

        a.goto(254,0,218,0,delay = 2000)

        if auto : 
            time.sleep(1)
        else :
            input()

        
finally :
    s.close()
